﻿using System;
using System.Collections.Generic;
using System.Text;

namespace G3.State
{
    class Car
    {
        private double weight = 1000.0; //kg
        private double throttleDeadzone = 0.01;
        private double engineStrength = 0.1;
        private double brakeStrength = 0.1;
        private double steerStrength = 0.001;
        private double steerSlowingRatio = 8;

        public double Weight
        {
            get
            {
                return weight;
            }
            set
            {
                weight = value;
            }
        }

        public Pair WheelPosition(double orientation, int wheelNumber)
        {
            return new Pair(0.0, 0.0);
        }

        public Pair Force(double throttle, double wheel, double orientation, double speed, G3.Controller.Field f) // m/ms2
        {
            Pair p = new Pair(0.0, 0.0); // Newton
            p.x = speed * speed * wheel * steerStrength;
            if (p.x > f.grip * speed) p.x = f.grip * speed;
            if (p.x < -f.grip * speed) p.x = -f.grip * speed;
            if (Math.Abs(throttle) < throttleDeadzone) p.y = 0.0;
            else if (throttle < 0.0)
            {
                p.y = brakeStrength * throttle;
            }
            else
            {
                p.y = engineStrength * throttle;
            }
            p.y -= Math.Abs(p.x)/steerSlowingRatio;
            p = (1000.0 / weight) * p; // Newton to m/ms2
            p.y -= speed * speed * (f.friction/10);
            return p;
        }

        public Pair InputFromAccel(Pair a, double speed, G3.Controller.Field f)
        {
            double x = a.x;
            double y = a.y;
            double s = speed;
            double gamma1 = steerStrength;
            double gamma2 = engineStrength; //TODO with brakeStrength
            double alpha = 1000.0 / weight;
            double beta = f.friction / 10.0;
            double delta = steerSlowingRatio;
            double w = x / (s * s * gamma1 * alpha);
            double t = (1.0 / (gamma2 * alpha)) * (y + s * s * beta + x / delta);
            Pair p = new Pair(w, t);
            return p;
        }
    }
}
